/*
 * Robot.h
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */

#ifndef ROBOT_H_
#define ROBOT_H_

#define LEGS 4

#include "Leg.h"
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
using std::vector;
using std::endl;
using std::cout;
using namespace std;

class Robot {
public:
	Robot() {
		legs = new Leg[LEGS];
		x = 0;
		y = 0;
		z = 0;
		speed = 0;
		oldspeed = 0;
		width = 0;
		length = 0;
		height = 0;
		vl = 0;
		fl = 0;
		hl = 0;
		bl = 0;
		Robot::setOrientation(5);
		logbool=0;
	}

	virtual ~Robot() {
		delete[] legs;
		legs = NULL;
	}

	double getHeight() const { return height; }
	double getLength() const { return length; }
	double getOldspeed() const { return oldspeed; }
	double getSpeed() const { return speed; }
	double getWidth() const { return width; }
	double getX() const { return x; }
	double getY() const { return y; }
	double getZ() const { return z; }
	void setHeight(double height) { this->height = height; }
	void setLength(double length) { this->length = length; }
	void setOldspeed(double oldspeed) { this->oldspeed = oldspeed; }
	void setSpeed(double speed) { this->speed = speed; }
	void setWidth(double width) { this->width = width; }
	void setX(double x) { this->x = x; }
	void setY(double y) { this->y = y; }
	void setZ(double z) { this->z = z; }

	void printInfo() {
		for (int i = 0; i < LEGS; i++) {
			cout << "DesiredX: " << Robot::legs[i].getDesiredx() << endl;
			cout << "DesiredY: " << Robot::legs[i].getDesiredy() << endl;
			cout << "DesiredZ: " << Robot::legs[i].getDesiredz() << endl;

			for (int j = 0; j < LINKS; j++) {
				cout << "Leg " << i << " Link " << j << endl;
				Robot::legs[i].links[j].printInfo();
			}
		}

		string heading;

		switch (Robot::orientation) {
		case 1:
			heading = "STOP";
			break;
		case 3:
			heading = "RIGHT";
			break;
		case 4:
			heading = "LEFT";
			break;
		case 5:
			heading = "FORWARD";
			break;
		case 6:
			heading = "BACKWARD";
			break;
		}
		cout << "Robot's heading: " << heading << endl;
		cout << vl << " " << fl << " " << bl << " " << hl << endl;
	}

	double getHl() const { return hl; }
	void setHl(double hl) { this->hl = hl; }
	double getBl() const { return bl; }
	double getFl() const { return fl; }
	double getVl() const { return vl; }
	void setBl(double bl) { this->bl = bl; }
	void setFl(double fl) { this->fl = fl; }
	void setVl(double vl) { this->vl = vl; }
	int getOrientation() const { return orientation; }

	void setOrientation(int orientation) {
		this->orientation = orientation;
		switch (orientation) {
		//Translates forward
		case 5:
			vl = 1.0;
			fl = 0.0;
			bl = 0.0;
			hl = 1.0;
			break;
			//Translates backwards
		case 6:
			vl = -1.0;
			fl = 0.0;
			bl = 0.0;
			hl = 1.0;
			break;
			//Translates left
		case 4:
			vl = 0.0;
			fl = -1.0;
			bl = -1.0;
			hl = 1.0;
			break;
			//Translates right
		case 3:
			vl = 0.0;
			fl = 1.0;
			bl = 1.0;
			hl = 1.0;
			break;
			//STOP
		case 1:
			vl = 0.0;
			fl = 0.0;
			bl = 0.0;
			hl = 0.0;
			break;
		}
	}

	Leg* legs;

	Matrix forwardK() {
		Matrix C = Matrix(4, 3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
		for (int i = 0; i < LEGS; i++) {
			Matrix A = Robot::legs[i].forwardK();
			C.setvalue(i, 0, A.getvalue(0, 3));
			C.setvalue(i, 1, A.getvalue(1, 3));
			C.setvalue(i, 2, A.getvalue(2, 3));
		}
		//cout << "ROBOT: " << C << endl;
		return C;
	}

	void inverseK() {
		for (int i = 0; i < LEGS; i++) {
			Matrix A = Robot::legs[i].inverseK();

			//POSITIONS

			//Set old Q values
			Robot::legs[i].links[0].setOld(Robot::legs[i].links[0].getQ());
			Robot::legs[i].links[1].setOld(Robot::legs[i].links[1].getQ());
			Robot::legs[i].links[2].setOld(Robot::legs[i].links[2].getQ());
			//Set new Q values
			Robot::legs[i].links[0].setQ(A.getvalue(0, 0));
			Robot::legs[i].links[1].setQ(A.getvalue(0, 1));
			Robot::legs[i].links[2].setQ(A.getvalue(0, 2));

			//VELOCITIES

			//Set old dQ values
			Robot::legs[i].links[0].setDq_old(Robot::legs[i].links[0].getDq());
			Robot::legs[i].links[1].setDq_old(Robot::legs[i].links[1].getDq());
			Robot::legs[i].links[2].setDq_old(Robot::legs[i].links[2].getDq());
			//Set new dQ values
			Robot::legs[i].links[0].setDq(Robot::legs[i].links[0].getQ()
					- Robot::legs[i].links[0].getOld());
			Robot::legs[i].links[1].setDq(Robot::legs[i].links[1].getQ()
					- Robot::legs[i].links[1].getOld());
			Robot::legs[i].links[2].setDq(Robot::legs[i].links[2].getQ()
					- Robot::legs[i].links[2].getOld());

			//ACCELERATIONS

			//Set old ddQ values
			Robot::legs[i].links[0].setDdq_old(Robot::legs[i].links[0].getDdq());
			Robot::legs[i].links[1].setDdq_old(Robot::legs[i].links[1].getDdq());
			Robot::legs[i].links[2].setDdq_old(Robot::legs[i].links[2].getDdq());
			//Set new ddQ values
			Robot::legs[i].links[0].setDdq(Robot::legs[i].links[0].getDq()
					- Robot::legs[i].links[0].getDq_old());
			Robot::legs[i].links[1].setDdq(Robot::legs[i].links[1].getDq()
					- Robot::legs[i].links[1].getDq_old());
			Robot::legs[i].links[2].setDdq(Robot::legs[i].links[2].getDq()
					- Robot::legs[i].links[2].getDq_old());

		}
	}

	void printIK() {
		for (int i = 0; i < LEGS; i++) {
			cout << "Leg " << i << " Q: " << Robot::legs[i].links[0].getQ() << " "
					<< Robot::legs[i].links[1].getQ() << " "
					<< Robot::legs[i].links[2].getQ() << endl;
			cout << "Leg " << i << " dQ: " << Robot::legs[i].links[0].getDq()
					<< " " << Robot::legs[i].links[1].getDq() << " "
					<< Robot::legs[i].links[2].getDq() << endl;
			cout << "Leg " << i << " ddQ: " << Robot::legs[i].links[0].getDdq()
					<< " " << Robot::legs[i].links[1].getDdq() << " "
					<< Robot::legs[i].links[2].getDdq() << endl;

		}
	}

	void logIK() {
		ofstream mylog;

		if (logbool == 0) {
			mylog.open("logIK.txt", ios::out | ios::trunc);
			logbool = 1;
		} else {
			mylog.open("logIK.txt", ios::out | ios::app);
		}
		for(int i=0;i<LEGS;i++){
		mylog << Robot::legs[i].links[0].getQ() << endl
				<< Robot::legs[i].links[1].getQ() << endl
				<< Robot::legs[i].links[2].getQ() << endl
				<< Robot::legs[i].links[0].getDq() << endl
				<< Robot::legs[i].links[1].getDq() << endl
				<< Robot::legs[i].links[2].getDq() << endl
				<< Robot::legs[i].links[0].getDdq() << endl
				<< Robot::legs[i].links[1].getDdq() << endl
				<< Robot::legs[i].links[2].getDdq() << endl;
		}

		mylog.close();
	}

private:
	int logbool;
	double x;
	double y;
	double z;
	double speed;
	double oldspeed;
	double width;
	double length;
	double height;
	int orientation;
	double fl;
	double bl;
	double vl;
	double hl;
};

#endif
